import CONSTANT from "../constant";

export default class Island {
    constructor() {
        this.rigidBodies = new Array(CONSTANT.SETTING_ISLAND_INITIAL_RIGID_BODY_ARRAY_SIZE);
        this.solvers = new Array(CONSTANT.SETTING_ISLAND_INITIAL_CONSTRAINT_ARRAY_SIZE);
        this.solversSi = new Array(CONSTANT.SETTING_ISLAND_INITIAL_CONSTRAINT_ARRAY_SIZE);
        this.solversNgs = new Array(CONSTANT.SETTING_ISLAND_INITIAL_CONSTRAINT_ARRAY_SIZE);
        this.numRigidBodies = 0;
        this.numSolvers = 0;
        this.numSolversSi = 0;
        this.numSolversNgs = 0;
        this.gravity=new Float64Array(3);
    }
    _clear() {
        while(this.numRigidBodies > 0) this.rigidBodies[--this.numRigidBodies] = null;
        while(this.numSolvers > 0) this.solvers[--this.numSolvers] = null;
        while(this.numSolversSi > 0) this.solversSi[--this.numSolversSi] = null;
        while(this.numSolversNgs > 0) this.solversNgs[--this.numSolversNgs] = null;
    }
    _addRigidBody(rigidBody) {
        if(this.numRigidBodies === this.rigidBodies.length) {
            let newArray = new Array(this.numRigidBodies << 1);
            let _g = 0, _g1 = this.numRigidBodies;
            while(_g < _g1) {
                let i = _g++;
                newArray[i] = this.rigidBodies[i];
                this.rigidBodies[i] = null;
            }
            this.rigidBodies = newArray;
        }
        rigidBody._addedToIsland = true;
        this.rigidBodies[this.numRigidBodies++] = rigidBody;
    }
    _addConstraintSolver(solver,positionCorrection) {
        if(this.numSolvers === this.solvers.length) {
            let newArray = new Array(this.numSolvers << 1);
            let _g = 0, _g1 = this.numSolvers;
            while(_g < _g1) {
                let i = _g++;
                newArray[i] = this.solvers[i];
                this.solvers[i] = null;
            }
            this.solvers = newArray;
        }
        solver._addedToIsland = true;
        this.solvers[this.numSolvers++] = solver;
        if(positionCorrection === CONSTANT.POSITION_CORRECTION_ALGORITHM_SPLIT_IMPULSE) {
            if(this.numSolversSi === this.solversSi.length) {
                let newArray = new Array(this.numSolversSi << 1);
                let _g = 0, _g1 = this.numSolversSi;
                while(_g < _g1) {
                    let i = _g++;
                    newArray[i] = this.solversSi[i];
                    this.solversSi[i] = null;
                }
                this.solversSi = newArray;
            }
            this.solversSi[this.numSolversSi++] = solver;
        }
        if(positionCorrection === CONSTANT.POSITION_CORRECTION_ALGORITHM_NGS) {
            if(this.numSolversNgs === this.solversNgs.length) {
                let newArray = new Array(this.numSolversNgs << 1);
                let _g = 0, _g1 = this.numSolversNgs;
                while(_g < _g1) {
                    let i = _g++;
                    newArray[i] = this.solversNgs[i];
                    this.solversNgs[i] = null;
                }
                this.solversNgs = newArray;
            }
            this.solversNgs[this.numSolversNgs++] = solver;
        }
    }
    _stepSingleRigidBody(timeStep,rb) {
        const SVT=CONSTANT.SETTING_SLEEPING_VELOCITY_THRESHOLD;
        const SAT=CONSTANT.SETTING_SLEEPING_ANGULAR_VELOCITY_THRESHOLD;
        const STT=CONSTANT.SETTING_SLEEPING_TIME_THRESHOLD;
        const RBD=CONSTANT.RIGID_BODY_TYPE_DYNAMIC;
        let dt = timeStep.dt;
        let dst = rb._ptransform.elements;
        let src = rb._transform.elements;
        let lci=rb.linearContactImpulse,aci=rb.angularContactImpulse,vel=rb.vel,angVel=rb.angVel,force=rb.force,torque=rb.torque,im=rb._invMass,ii=rb.invInertia;
        let gt=this.gravity,gs=rb._gravityScale;
        dst[0] = src[0];dst[1] = src[1];dst[2] = src[2];
        dst[3] = src[3];dst[4] = src[4];dst[5] = src[5];
        dst[6] = src[6];dst[7] = src[7];dst[8] = src[8];
        dst[9] = src[9];dst[10] = src[10];dst[11] = src[11];
        lci[0] = lci[1] = lci[2] = 0;
        aci[0] = aci[1] = aci[2] = 0;
        if(rb._autoSleep && vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2] < SVT  * SVT  &&
            angVel[0] * angVel[0] + angVel[1] * angVel[1] + angVel[2] * angVel[2] < SAT  * SAT) {
            rb._sleepTime += dt;
            if(rb._sleepTime > STT ) {
                rb._sleeping = true;
                rb._sleepTime = 0;
            }
        } else {
            rb._sleepTime = 0;
        }
        if(!rb._sleeping) {
            if(rb._type === RBD) {
                let x = dt * rb._linearDamping;
                let x2 = x * x;
                let linScale = 1 / (1 + x + x2 * (0.5 + x * 0.16666666666666666 + x2 * 0.041666666666666664));
                let x1 = dt * rb._angularDamping;
                let x21 = x1 * x1;
                let angScale = 1 / (1 + x1 + x21 * (0.5 + x1 * 0.16666666666666666 + x21 * 0.041666666666666664));
                let linAccX = gt[0] * gs,linAccY = gt[1] * gs,linAccZ = gt[2] * gs;
                linAccX += force[0] * im;linAccY += force[1] * im;linAccZ += force[2] * im;
                let angAccX = ii[0] * torque[0] + ii[1] * torque[1] + ii[2] * torque[2];
                let angAccY = ii[3] * torque[0] + ii[4] * torque[1] + ii[5] * torque[2];
                let angAccZ = ii[6] * torque[0] + ii[7] * torque[1] + ii[8] * torque[2];
                vel[0] += linAccX * dt;vel[1] += linAccY * dt;vel[2] += linAccZ * dt;
                vel[0] *= linScale;vel[1] *= linScale;vel[2] *= linScale;
                angVel[0] += angAccX * dt;angVel[1] += angAccY * dt;angVel[2] += angAccZ * dt;
                angVel[0] *= angScale;angVel[1] *= angScale;angVel[2] *= angScale;
            }
            rb._integrate(dt);
            let s = rb._shapeList;
            while(s != null) {
                let n = s._next;
                let tf1 = rb._ptransform.elements;
                let tf2 = rb._transform.elements;
                let dst = s._ptransform.elements;
                let src1 = s._localTransform.elements;
                dst[3] = tf1[3] * src1[3] + tf1[4] * src1[6] + tf1[5] * src1[9];
                dst[4] = tf1[3] * src1[4] + tf1[4] * src1[7] + tf1[5] * src1[10];
                dst[5] = tf1[3] * src1[5] + tf1[4] * src1[8] + tf1[5] * src1[11];
                dst[6] = tf1[6] * src1[3] + tf1[7] * src1[6] + tf1[8] * src1[9];
                dst[7] = tf1[6] * src1[4] + tf1[7] * src1[7] + tf1[8] * src1[10];
                dst[8] = tf1[6] * src1[5] + tf1[7] * src1[8] + tf1[8] * src1[11];
                dst[9] = tf1[9] * src1[3] + tf1[10] * src1[6] + tf1[11] * src1[9];
                dst[10] = tf1[9] * src1[4] + tf1[10] * src1[7] + tf1[11] * src1[10];
                dst[11] = tf1[9] * src1[5] + tf1[10] * src1[8] + tf1[11] * src1[11];
                dst[0] = tf1[3] * src1[0] + tf1[4] * src1[1] + tf1[5] * src1[2];
                dst[1] = tf1[6] * src1[0] + tf1[7] * src1[1] + tf1[8] * src1[2];
                dst[2] = tf1[9] * src1[0] + tf1[10] * src1[1] + tf1[11] * src1[2];
                dst[0] += tf1[0];dst[1] += tf1[1];dst[2] += tf1[2];
                let dst1 = s._transform.elements;
                let src11 = s._localTransform.elements;
                dst1[3] = tf2[3] * src11[3] + tf2[4] * src11[6] + tf2[5] * src11[9];
                dst1[4] = tf2[3] * src11[4] + tf2[4] * src11[7] + tf2[5] * src11[10];
                dst1[5] = tf2[3] * src11[5] + tf2[4] * src11[8] + tf2[5] * src11[11];
                dst1[6] = tf2[6] * src11[3] + tf2[7] * src11[6] + tf2[8] * src11[9];
                dst1[7] = tf2[6] * src11[4] + tf2[7] * src11[7] + tf2[8] * src11[10];
                dst1[8] = tf2[6] * src11[5] + tf2[7] * src11[8] + tf2[8] * src11[11];
                dst1[9] = tf2[9] * src11[3] + tf2[10] * src11[6] + tf2[11] * src11[9];
                dst1[10] = tf2[9] * src11[4] + tf2[10] * src11[7] + tf2[11] * src11[10];
                dst1[11] = tf2[9] * src11[5] + tf2[10] * src11[8] + tf2[11] * src11[11];
                dst1[0] = tf2[3] * src11[0] + tf2[4] * src11[1] + tf2[5] * src11[2];
                dst1[1] = tf2[6] * src11[0] + tf2[7] * src11[1] + tf2[8] * src11[2];
                dst1[2] = tf2[9] * src11[0] + tf2[10] * src11[1] + tf2[11] * src11[2];
                dst1[0] += tf2[0];dst1[1] += tf2[1];dst1[2] += tf2[2];
                s._geom._computeAabb(s._aabb,s._ptransform);
                let ab=s._aabb.elements;
                let minX = ab[0],minY = ab[1],minZ = ab[2];
                let maxX = ab[3],maxY = ab[4],maxZ = ab[5];
                s._geom._computeAabb(s._aabb,s._transform);
                ab[0] = minX < ab[0] ? minX : ab[0];ab[1] = minY < ab[1] ? minY : ab[1];ab[2] = minZ < ab[2] ? minZ : ab[2];
                ab[3] = maxX > ab[3] ? maxX : ab[3];ab[4] = maxY > ab[4] ? maxY : ab[4];ab[5] = maxZ > ab[5] ? maxZ : ab[5];
                if(s._proxy != null) {
                    let v = s.displacement.elements,transform=s._transform.elements,ptransform=s._ptransform.elements;
                    v[0] = transform[0] - ptransform[0];v[1] = transform[1] - ptransform[1];v[2] = transform[2] - ptransform[2];
                    s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
                }
                s = n;
            }
        }
    }
    _step(timeStep,numVelocityIterations,numPositionIterations) {
        const SVT=CONSTANT.SETTING_SLEEPING_VELOCITY_THRESHOLD;
        const SAT=CONSTANT.SETTING_SLEEPING_ANGULAR_VELOCITY_THRESHOLD;
        const SLT=CONSTANT.SETTING_SLEEPING_TIME_THRESHOLD;
        const BTD=CONSTANT.RIGID_BODY_TYPE_DYNAMIC;
        let dt = timeStep.dt;
        let sleepIsland = true,gt=this.gravity;
        let _g = 0, _g1 = this.numRigidBodies;
        while(_g < _g1) {
            let rb = this.rigidBodies[_g++];
            let dst = rb._ptransform.elements, src = rb._transform.elements;
            let vel=rb.vel,angVel=rb.angVel,gvs=rb._gravityScale,invMass=rb._invMass,force=rb.force,ii=rb.invInertia,torque=rb.torque;
            let linearContactImpulse=rb.linearContactImpulse;
            let angularContactImpulse=rb.angularContactImpulse;
            dst[0] = src[0];dst[1] = src[1];dst[2] = src[2];dst[3] = src[3];dst[4] = src[4];dst[5] = src[5];dst[6] = src[6];dst[7] = src[7];dst[8] = src[8];dst[9] = src[9];dst[10] = src[10];dst[11] = src[11];
            linearContactImpulse[0]=linearContactImpulse[1]=linearContactImpulse[2] = 0;
            angularContactImpulse[0]=angularContactImpulse[1]=angularContactImpulse[2] = 0;
            rb._sleeping = false;
            if(rb._autoSleep && vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2] < SVT  * SVT &&
                angVel[0] * angVel[0] + angVel[1] * angVel[1] + angVel[2] * angVel[2] < SAT  * SAT) {
                rb._sleepTime += dt;
            } else {
                rb._sleepTime = 0;
            }
            if(rb._sleepTime < SLT ) {
                sleepIsland = false;
            }
            if(rb._type === BTD) {
                let x = dt * rb._linearDamping;
                let x2 = x * x;
                let linScale = 1 / (1 + x + x2 * (0.5 + x * 0.16666666666666666 + x2 * 0.041666666666666664));
                let x1 = dt * rb._angularDamping;
                let x21 = x1 * x1;
                let angScale = 1 / (1 + x1 + x21 * (0.5 + x1 * 0.16666666666666666 + x21 * 0.041666666666666664));
                let linAccX = gt[0] * gvs,linAccY = gt[1] * gvs,linAccZ = gt[2] * gvs;
                linAccX += force[0] * invMass;linAccY += force[1] * invMass;linAccZ += force[2] * invMass;
                let angAccX = ii[0] * torque[0] + ii[1] * torque[1] + ii[2] * torque[2];
                let angAccY = ii[3] * torque[0] + ii[4] * torque[1] + ii[5] * torque[2];
                let angAccZ = ii[6] * torque[0] + ii[7] * torque[1] + ii[8] * torque[2];
                vel[0] += linAccX * dt;vel[1] += linAccY * dt;vel[2] += linAccZ * dt;
                vel[0] *= linScale;vel[1] *= linScale;vel[2] *= linScale;
                angVel[0] += angAccX * dt;angVel[1] += angAccY * dt;angVel[2] += angAccZ * dt;
                angVel[0] *= angScale;angVel[1] *= angScale;angVel[2] *= angScale;
            }
        }
        if(sleepIsland) {
            let _g = 0, _g1 = this.numRigidBodies;
            while(_g < _g1) {
                let rb = this.rigidBodies[_g++];
                rb._sleeping = true;
                rb._sleepTime = 0;
            }
            return;
        }
        let _g2 = 0, _g3 = this.numSolvers;
        while(_g2 < _g3) this.solvers[_g2++].preSolveVelocity(timeStep);
        let _g4 = 0, _g5 = this.numSolvers;
        while(_g4 < _g5) this.solvers[_g4++].warmStart(timeStep);
        let _g6 = 0;
        while(_g6 < numVelocityIterations) {
            ++_g6;
            let _g = 0, _g1 = this.numSolvers;
            while(_g < _g1) this.solvers[_g++].solveVelocity();
        }
        let _g7 = 0, _g8 = this.numSolvers;
        while(_g7 < _g8) this.solvers[_g7++].postSolveVelocity(timeStep);
        let _g9 = 0, _g10 = this.numRigidBodies;
        while(_g9 < _g10) this.rigidBodies[_g9++]._integrate(dt);
        let _g11 = 0, _g12 = this.numSolversSi;
        while(_g11 < _g12) this.solversSi[_g11++].preSolvePosition(timeStep);
        let _g13 = 0;
        while(_g13 < numPositionIterations) {
            ++_g13;
            let _g = 0, _g1 = this.numSolversSi;
            while(_g < _g1) this.solversSi[_g++].solvePositionSplitImpulse();
        }
        let _g14 = 0, _g15 = this.numRigidBodies;
        while(_g14 < _g15) this.rigidBodies[_g14++]._integratePseudoVelocity();
        let _g16 = 0, _g17 = this.numSolversNgs;
        while(_g16 < _g17) this.solversNgs[_g16++].preSolvePosition(timeStep);
        let _g18 = 0;
        while(_g18 < numPositionIterations) {
            ++_g18;
            let _g = 0, _g1 = this.numSolversNgs;
            while(_g < _g1) this.solversNgs[_g++].solvePositionNgs(timeStep);
        }
        let _g19 = 0, _g20 = this.numSolvers;
        while(_g19 < _g20) this.solvers[_g19++].postSolve();
        let _g21 = 0, _g22 = this.numRigidBodies;
        while(_g21 < _g22) {
            let rb = this.rigidBodies[_g21++];
            let s = rb._shapeList;
            while(s != null) {
                let n = s._next;
                let tf1 = rb._ptransform.elements;
                let tf2 = rb._transform.elements;
                let dst = s._ptransform.elements;
                let src1 = s._localTransform.elements;
                dst[3] = tf1[3] * src1[3] + tf1[4] * src1[6] + tf1[5] * src1[9];
                dst[4] = tf1[3] * src1[4] + tf1[4] * src1[7] + tf1[5] * src1[10];
                dst[5] = tf1[3] * src1[5] + tf1[4] * src1[8] + tf1[5] * src1[11];
                dst[6] = tf1[6] * src1[3] + tf1[7] * src1[6] + tf1[8] * src1[9];
                dst[7] = tf1[6] * src1[4] + tf1[7] * src1[7] + tf1[8] * src1[10];
                dst[8] = tf1[6] * src1[5] + tf1[7] * src1[8] + tf1[8] * src1[11];
                dst[9] = tf1[9] * src1[3] + tf1[10] * src1[6] + tf1[11] * src1[9];
                dst[10] = tf1[9] * src1[4] + tf1[10] * src1[7] + tf1[11] * src1[10];
                dst[11] = tf1[9] * src1[5] + tf1[10] * src1[8] + tf1[11] * src1[11];
                dst[0] = tf1[3] * src1[0] + tf1[4] * src1[1] + tf1[5] * src1[2];
                dst[1] = tf1[6] * src1[0] + tf1[7] * src1[1] + tf1[8] * src1[2];
                dst[2] = tf1[9] * src1[0] + tf1[10] * src1[1] + tf1[11] * src1[2];
                dst[0] += tf1[0];dst[1] += tf1[1];dst[2] += tf1[2];
                let dst1 = s._transform.elements;
                let src11 = s._localTransform.elements;
                dst1[3] = tf2[3] * src11[3] + tf2[4] * src11[6] + tf2[5] * src11[9];
                dst1[4] = tf2[3] * src11[4] + tf2[4] * src11[7] + tf2[5] * src11[10];
                dst1[5] = tf2[3] * src11[5] + tf2[4] * src11[8] + tf2[5] * src11[11];
                dst1[6] = tf2[6] * src11[3] + tf2[7] * src11[6] + tf2[8] * src11[9];
                dst1[7] = tf2[6] * src11[4] + tf2[7] * src11[7] + tf2[8] * src11[10];
                dst1[8] = tf2[6] * src11[5] + tf2[7] * src11[8] + tf2[8] * src11[11];
                dst1[9] = tf2[9] * src11[3] + tf2[10] * src11[6] + tf2[11] * src11[9];
                dst1[10] = tf2[9] * src11[4] + tf2[10] * src11[7] + tf2[11] * src11[10];
                dst1[11] = tf2[9] * src11[5] + tf2[10] * src11[8] + tf2[11] * src11[11];
                dst1[0] = tf2[3] * src11[0] + tf2[4] * src11[1] + tf2[5] * src11[2];
                dst1[1] = tf2[6] * src11[0] + tf2[7] * src11[1] + tf2[8] * src11[2];
                dst1[2] = tf2[9] * src11[0] + tf2[10] * src11[1] + tf2[11] * src11[2];
                dst1[0] += tf2[0];dst1[1] += tf2[1];dst1[2] += tf2[2];
                s._geom._computeAabb(s._aabb,s._ptransform);
                let ab=s._aabb.elements;
                let minX = ab[0],minY = ab[1],minZ = ab[2];
                let maxX = ab[3],maxY = ab[4],maxZ = ab[5];
                s._geom._computeAabb(s._aabb,s._transform);
                ab[0] = minX < ab[0] ? minX : ab[0];ab[1] = minY < ab[1] ? minY : ab[1];ab[2] = minZ < ab[2] ? minZ : ab[2];
                ab[3] = maxX > ab[3] ? maxX : ab[3];ab[4] = maxY > ab[4] ? maxY : ab[4];ab[5] = maxZ > ab[5] ? maxZ : ab[5];
                if(s._proxy != null) {
                    let v = s.displacement.elements,transform=s._transform.elements,ptransform=s._ptransform.elements;
                    v[0] = transform[0] - ptransform[0];v[1] = transform[1] - ptransform[1];v[2] = transform[2] - ptransform[2];
                    s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
                }
                s = n;
            }
        }
    }
}